Robust Online Motion Planning with Regions of Finite Time Invariance

In this paper we consider the problem of generating motion plans for a nonlinear dynamical system that are guaranteed to succeed despite uncertainty in the environment, parametric model uncertainty, disturbances, and/or errors in state estimation. Furthermore, we consider the case where these plans must be generated online, because constraints such as obstacles in the environment may not be known until they are perceived (with a noisy sensor) at runtime. Previous work on feedback motion planning for nonlinear systems was limited to offline planning due to the computational cost of safety verification. Here we take a trajectory library approach by designing controllers that stabilize the nominal trajectories in the library and precomputing regions of finite time invariance (“funnels”) for the resulting closed loop system. We leverage sums-of-squares programming in order to efficiently compute funnels which take into account bounded disturbances and uncertainty. The resulting funnel library is then used to sequentially compose motion plans at runtime while ensuring the safety of the robot. A major advantage of the work presented here is that by explicitly taking into account the effect of uncertainty, the robot can evaluate motion plans based on how vulnerable they are to disturbances. We demonstrate our method on a simulation of a plane flying through a two dimensional forest of polygonal trees with parametric uncertainty and disturbances in the form of a bounded “cross-wind”.